import java.beans.XMLDecoder
import com.sun.j3d.utils.universe.SimpleUniverse
import javax.media.j3d.*
import javax.vecmath.*
import java.awt.Color

public class Arduino3D extends Shape3D {
	public LoadShape () {

	}
	Shape3D loadShape(String filename) {
		def de = new XMLDecoder(
					new BufferedInputStream(
	                       new FileInputStream(filename)))
	    def ls = de.readObject()
	    de.close()
		ls
	}
	public static void main (String[] args) {
		def universe = new SimpleUniverse()
		def group = new BranchGroup()
		def Arduino3Ds = []
		def ls1 = new Arduino3D().loadShape(args[0])
		def ls2 = new Arduino3D().loadShape(args[1])
//		group.addChild(ls1)
		def jawTg = new TransformGroup()
		jawTg.setCapability(TransformGroup.ALLOW_TRANSFORM_WRITE)
		jawTg.addChild(ls1)
		group.addChild(jawTg)
		Light light = new DirectionalLight(new Color3f(Color.BLUE), new Vector3f(1, -1, -1))
		light.setInfluencingBounds(new BoundingSphere())
		group.addChild(light)
		universe.addBranchGraph(group)
		def t3d = universe.getViewingPlatform().getViewPlatformTransform()
		def t1 = new Transform3D()
		t3d.getTransform(t1)
		def jawTransform = new Transform3D()
		jawTg.getTransform(jawTransform)
		t1.setTranslation new Vector3f(0.0f, 1.0f, 19f)
		t3d.setTransform t1
		
    try{
        Transform3D X = new Transform3D();
        Transform3D Y = new Transform3D();
        Transform3D Z = new Transform3D();
        Transform3D tmp=jawTransform;
        def d2r= 57.2957795
        double lx=0,ly=0,lz=0.0, nx=0.0,ny=0.0,nz=0.0;
        def minalpha=1.0/d2r
        Matrix3d m=new Matrix3d(1,       0,0,
                                       0,1,0,
                                       0,0,1);
                                       
        (new SimpleRead()).connect("/dev/ttyUSB0",{data->
          def pos=data.split(',')
          try{
            nx=((pos[0] as double)/d2r);
            ny=((pos[1] as double)/d2r);
            nz=((pos[2] as double)/d2r);
            if(lx==ly==lz==0.0){
              lx=nx;ly=ny;lz=nz;
            }
            
            if (Math.abs(lx-nx)>minalpha){
              X.rotX (nx-lx)
              lx=nx
        			jawTransform.mul X
            }
            if (Math.abs(ly-ny)>minalpha){
              Y.rotY (ny-ly)
              ly=ny
        			jawTransform.mul Y
            }
            if (Math.abs(lz-nz)>minalpha){
              Z.rotZ (nz-lz)
              lz=nz
        			jawTransform.mul Z
            }
    			
    			  jawTg.setTransform jawTransform
    			  
    			
    			System.out.println(pos);
			    Thread.sleep 20
//			    X = new Transform3D();Y = new Transform3D();Z = new Transform3D();
			    }catch(java.lang.NumberFormatException ne){
			    
			    
          }catch(Exception e){
            e.printStackTrace();
            System.exit(-1);
          }
          
        });
    }
    catch ( Exception e )
    {
        e.printStackTrace();
    }
		
		50.times{
			3.times { angle ->
			jawTransform.rotY (angle / 5.0)
			jawTg.setTransform jawTransform
			Thread.sleep 200
			}
		}
	}
}

